#include "Physics.h"
#include "ShapeRectangle.h"
#include "ShapeCircle.h"
#include "ComponentManager.h"

#include <cstdio>

CPhysics::CPhysics(void)
{
	m_timeStart = (unsigned long)-1;
	m_currentTime = 0;
	m_pauseTime = 0;
	m_timePreviousUpdate = (unsigned long)-1;
	m_oneMeterIsInPixels = 80;
	m_bPause = false;
	m_timeStartPause = 0;
}

CPhysics::~CPhysics(void)
{
}

IShape* CPhysics::createRectangle(int width, int height)
{
	//create the rectangle shape
	IShape* newShape = new CShapeRectangle();
	newShape->m_typeOfShape = eRectangleShape;
	((CShapeRectangle*)newShape)->m_width = width;
	((CShapeRectangle*)newShape)->m_height = height;
	
	//calculate the bounding box
	((CShapeRectangle*)newShape)->calculateBoundingBox();

	//store the shape in the vector
	m_vectorOfPhysicEntities.push_back(newShape);

	//return th shape
	return newShape;
}

IShape* CPhysics::createCircle(int radius)
{
	//create the circle shape
	IShape* newShape = new CShapeCircle();
	newShape->m_typeOfShape = eCircleShape;
	((CShapeCircle*)newShape)->m_radius = radius;

	//store the shape
	m_vectorOfPhysicEntities.push_back(newShape);

	//return the shape
	return newShape;
}

void CPhysics::doUpdate()
{
	if(m_timeStart == -1)
	{
		m_timeStart = timeGetTime();
		m_timePreviousUpdate = timeGetTime();
	}

	//update position
	for(vector<IShape*>::iterator iterObject = m_vectorOfPhysicEntities.begin(); iterObject != m_vectorOfPhysicEntities.end(); iterObject++)
	{
		
		//if the object is a kinematic actor
		if((*iterObject)->m_bKinematic == true)
			continue;

		//get the weight
		float weight;
		weight = -(*iterObject)->m_mass*m_gravity;


		//calculate the sum of forces
		float sumOfForcesY = weight;
		float sumOfForcesX = 0;

		//calculate the time elapsed if we're not on pause
		if(!m_bPause)
			m_currentTime = timeGetTime() - m_timeStart - m_pauseTime;

		float currentTimeSecond = (float)m_currentTime/1000;

		float tSquare = currentTimeSecond*currentTimeSecond;

		//calculate acceleration 
		(*iterObject)->m_acceleration.x = sumOfForcesX / (*iterObject)->m_mass;
		(*iterObject)->m_acceleration.y = sumOfForcesY / (*iterObject)->m_mass;

		//calculate speed
		(*iterObject)->m_speed.x = (*iterObject)->m_initialSpeed.x + (*iterObject)->m_acceleration.x*currentTimeSecond;
		(*iterObject)->m_speed.y = (*iterObject)->m_initialSpeed.y + (*iterObject)->m_acceleration.y*currentTimeSecond;

		//calculate the displacement
		CVector2f newPosition;
		
		(*iterObject)->m_position.x = (*iterObject)->m_initialPosition.x + (*iterObject)->m_initialSpeed.x*currentTimeSecond + (*iterObject)->m_acceleration.x*tSquare/2;
		(*iterObject)->m_position.y = (*iterObject)->m_initialPosition.y + (*iterObject)->m_initialSpeed.y*currentTimeSecond + (*iterObject)->m_acceleration.y*tSquare/2;


		(*iterObject)->m_orientation = (*iterObject)->m_initialOrientation;
		//check quad tree to get the bucket

		CM.SCENE().updateEntity((*iterObject));

	}

	//check collision
	for(vector<IShape*>::iterator iterObject = m_vectorOfPhysicEntities.begin(); iterObject != m_vectorOfPhysicEntities.end(); iterObject++)
	{
		if((*iterObject)->m_bKinematic == true)
			continue;

		//check collision
		IShape* collidedShape = checkCollision((*iterObject));
		//IShape* collidedShape=0;

		if(collidedShape != 0)
		{
			//display collision report
			displayCollisionReport(collidedShape, *iterObject);	

			//put the collided object in kinematic (temporary)
			collidedShape->m_bKinematic = true;
			(*iterObject)->m_bKinematic = true;

			//put their velocity and acceleration to 0
			collidedShape->m_speed = CVector2f(0, 0);
			collidedShape->m_acceleration = CVector2f(0, 0);
			(*iterObject)->m_speed = CVector2f(0, 0);
			(*iterObject)->m_acceleration = CVector2f(0, 0);
		}
	}

		//update the time
		m_timePreviousUpdate = timeGetTime();
	
}
void CPhysics::pauseSimulation(bool bPause)
{
	if(m_bPause == bPause)
		return;

	m_bPause = bPause;
	if(bPause)
	{
		
		m_timeStartPause = timeGetTime();
	}
	else
	{
		m_pauseTime += timeGetTime() - m_timeStartPause;
		m_timeStartPause = 0;

	}
}

void CPhysics::fillQuadTree(void)
{
	for(vector<IShape*>::iterator i=m_vectorOfPhysicEntities.begin(); i!=m_vectorOfPhysicEntities.end(); i++)
	{
		CM.SCENE().addEntity((*i));
	}
}

IShape* CPhysics::checkCollision(IShape* entity)
{

#define USE_SCENE

#ifndef USE_SCENE
	//for every entity
	for(vector<IShape*>::iterator iterObject = m_vectorOfPhysicEntities.begin(); iterObject != m_vectorOfPhysicEntities.end(); iterObject++)
	{
#else
	vector<IShape*>* pntVector = 0;
	int id = 0;
	bool over = false;
	while(!over)
	{
		pntVector = CM.SCENE().getVector(entity, id);
		if(pntVector == 0)
			return 0;
		for(vector<IShape*>::iterator iterObject = pntVector->begin(); iterObject != pntVector->end(); iterObject++)
		{
#endif

		//if entity and iterator are the same
		if(entity == (*iterObject))
			continue;

		bool res = 0;
		if(entity->m_typeOfShape == eRectangleShape && (*iterObject)->m_typeOfShape == eRectangleShape)
		{
			res = checkCollisionBetweenRectangle((CShapeRectangle*)entity, (CShapeRectangle*)(*iterObject));
			if(res)
			{
				bool col = getCollisionPointBetweenRectangles((CShapeRectangle*)entity, (CShapeRectangle*)(*iterObject));
				if(col)
				{
					entity->m_collision = true;
					(*iterObject)->m_collision = true;
					return *iterObject;
				}
				
			}
			
		}
		else if(entity->m_typeOfShape == eCircleShape && (*iterObject)->m_typeOfShape == eRectangleShape)
		{
			res = checkCollisionBetweenRectangleAndCircle((CShapeRectangle*)(*iterObject), (CShapeCircle*)entity);
			if(res)
			{
				entity->m_collision = true;
				(*iterObject)->m_collision = true;

				getCollisionPointBetweenRectangleAndCircle((CShapeRectangle*)(*iterObject), (CShapeCircle*)entity);
				return *iterObject;
			}

		}
		else if(entity->m_typeOfShape == eRectangleShape && (*iterObject)->m_typeOfShape == eCircleShape)
		{
			res = checkCollisionBetweenRectangleAndCircle((CShapeRectangle*)entity, (CShapeCircle*)(*iterObject));
			if(res)
			{
				entity->m_collision = true;
				(*iterObject)->m_collision = true;

				getCollisionPointBetweenRectangleAndCircle((CShapeRectangle*)entity, (CShapeCircle*)(*iterObject));
				return *iterObject;
			}

		}
		else if(entity->m_typeOfShape == eCircleShape && (*iterObject)->m_typeOfShape == eCircleShape)
		{
			res = checkCollisionBetweenCircles((CShapeCircle*)(*iterObject), (CShapeCircle*)entity);
			if(res)
			{
				entity->m_collision = true;
				(*iterObject)->m_collision = true;

				getCollisionPointBetweenCircles((CShapeCircle*)(*iterObject), (CShapeCircle*)entity);
				return *iterObject;
			}

		}				
	}
#ifdef USE_SCENE
		id++;
}
#endif


	return 0;
}

bool CPhysics::checkCollisionBetweenRectangleAndCircle(CShapeRectangle* rectangle, CShapeCircle* circle)
{
	//get the orientied bounding box of the rectangle
	CVector2f* boundingBox = rectangle->getOrientedBoundingBox();

	//get a vector between the two centers
	CVector2f vectorBetweenCenter = rectangle->m_position - circle->m_position;
	vectorBetweenCenter.normalize();
	
	//get the min and max for the bounding box;
	float maxBB1;
	float minBB1;
	polygonProjection(vectorBetweenCenter, boundingBox, &minBB1, &maxBB1);

	//get min an dmax for the circle
	float maxC;
	float minC;
	circleProjection(vectorBetweenCenter, circle->m_position, (float)circle->m_radius, &minC, &maxC);

	//check if overlap
	if(!checkOverlap(minBB1, maxBB1, minC, maxC))
		//if it doesn't overlap, a separate axis exists so no collision
		return false;	

	//re check with the side of the bounding box
	for(int i=0; i<3; i++)
	{
		int id1 = i;
		int id2 = i+1;

		CVector2f edge = boundingBox[id2] - boundingBox[id1];
		CVector2f orthogonal( -edge.y, edge.x);
		orthogonal.normalize();

		//get the min and max for the bounding box;
		polygonProjection(orthogonal, boundingBox, &minBB1, &maxBB1);

		//get min an dmax for the circle
		circleProjection(orthogonal, circle->m_position, (float)circle->m_radius, &minC, &maxC);

		//check if overlap
		if(!checkOverlap(minBB1, maxBB1, minC, maxC))
			//if it doesn't overlap, a separate axis exists so no collision
			return false;	

	}

	return true;
}

bool CPhysics::checkCollisionBetweenCircles(CShapeCircle* circle1, CShapeCircle* circle2)
{
	//get the distance between the centers
	float distance = (circle1->m_position - circle2->m_position).getNorme();

	//add the radius
	float sumOfRadius = (float)(circle1->m_radius + circle2->m_radius);

	if(distance>sumOfRadius)
		return false;
	else
		return true;
}

bool CPhysics::checkCollisionBetweenRectangle(CShapeRectangle* entity1, CShapeRectangle* entity2)
{
	
	//get the oriented bounding box for the two entities
	CVector2f* boundingBox1;
	CVector2f* boundingBox2;

	boundingBox1 = entity1->getOrientedBoundingBox();
	boundingBox2 = entity2->getOrientedBoundingBox();

	//create an array containing only the edge that we will have to test (as the shapes are rectangles, we need to test 2 edges per shapes so 4 edges).
	CVector2f vectorOfEdgeToTest[6];
	vectorOfEdgeToTest[0] = boundingBox1[0];
	vectorOfEdgeToTest[1] = boundingBox1[1];
	vectorOfEdgeToTest[2] = boundingBox1[2];
	vectorOfEdgeToTest[3] = boundingBox2[0];
	vectorOfEdgeToTest[4] = boundingBox2[1];
	vectorOfEdgeToTest[5] = boundingBox2[2];
	//for every edge of the rectangle 1 (so BB1)
	//We do it for 2 edges because the two others are parallel to the two firsts.
	for(int i=0; i<5; i++)
	{
		//get the vector of the edge
		if(i == 2)
			i++;
		int id1 = i;
		int id2 = i+1;

		CVector2f edge = vectorOfEdgeToTest[id1] - vectorOfEdgeToTest[id2];

		//get the perpendicular vector
		CVector2f orthogonalLine(-edge.y, edge.x);
		orthogonalLine.normalize();

		//get the min and max for the bounding box 1;
		float maxBB1;
		float minBB1;
		polygonProjection(orthogonalLine, boundingBox1, &minBB1, &maxBB1);
		
		//get the min and max for bounding box 2
		float maxBB2;
		float minBB2;
		polygonProjection(orthogonalLine, boundingBox2, &minBB2, &maxBB2);
		
		//check if overlap
		if(!checkOverlap(minBB1, maxBB1, minBB2, maxBB2))
			//if it doesn't overlap, a separate axis exists so no collision
			return false;	
	} 

	//no separate axis found so collision
    return true;

}

void CPhysics::polygonProjection(const CVector2f& projectionLine, const CVector2f* boundingBox, float* min, float* max)
{
	//for every point of the bounding box, calculate it's projection on the line
	float BBPoint0 = projectionLine.dot(boundingBox[0]);
	float BBPoint1 = projectionLine.dot(boundingBox[1]);
	float BBPoint2 = projectionLine.dot(boundingBox[2]);
	float BBPoint3 = projectionLine.dot(boundingBox[3]);

	//get the maximum
	*max = getMaximum(BBPoint0, BBPoint1);
	*max = getMaximum(*max, BBPoint2);
	*max = getMaximum(*max, BBPoint3);

	//get the minimum
	*min = getMinimum(BBPoint0, BBPoint1);
	*min = getMinimum(*min, BBPoint2);
	*min = getMinimum(*min, BBPoint3);
}

void CPhysics::circleProjection(const CVector2f& projectionLine, const CVector2f& position, float radius, float* min, float* max)
{
	//get the extremum point of a diameter parallel to the projection line
	CVector2f test = projectionLine * radius;
	CVector2f sideOne = position + projectionLine * radius;
	CVector2f sideTwo = position - projectionLine * radius;

	//project the point on the line
	float projectionPointOne = projectionLine.dot(sideOne);
	float projectionPointTwo = projectionLine.dot(sideTwo);

	//get the min and max
	*min = getMinimum(projectionPointOne, projectionPointTwo);
	*max = getMaximum(projectionPointOne, projectionPointTwo);
}

bool CPhysics::checkOverlap(float a1, float a2, float b1, float b2)
{
	float maxA = getMaximum(a1, a2);
	float minA = getMinimum(a1, a2);

	float maxB = getMaximum(b1, b2);
	float minB = getMinimum(b1, b2);

	if(maxA<minB || maxB< minA)
		return false;

	return true;

}

float CPhysics::getMaximum(float a, float b)
{
	if(a>b)
		return a;
	else
		return b;
}

float CPhysics::getMinimum(float a, float b)
{
	if(a<b)
		return a;
	else
		return b;
}

int CPhysics::meterToPixel(const float meter)
{
	return (int)(meter*m_oneMeterIsInPixels);
}

float CPhysics::pixelToMeter(const int pixel)
{
	return (float)pixel/m_oneMeterIsInPixels;
}

void CPhysics::displayCollisionReport(const IShape* entity1, const IShape* entity2)
{
	//pause the simulation
	pauseSimulation(true);

	//create the string for entity1
	string sEntity1;
	
	//calculate speed and acceleration
	float acceleration = sqrt(entity1->m_acceleration.x*entity1->m_acceleration.x+entity1->m_acceleration.y*entity1->m_acceleration.y);
	float speed = sqrt(entity1->m_speed.x*entity1->m_speed.x+entity1->m_speed.y*entity1->m_speed.y);

	//add the name
	sEntity1 = "Collision Report : \n\n";
	sEntity1.append(entity1->m_name);

	//add the acceleration
	char str[255];
	sprintf_s(str, "%.4g", acceleration);
	sEntity1.append(" :\nAcceleration = ");
	sEntity1.append(str);
	sEntity1.append(" m/s2");

	//add the speed
	sEntity1.append("\nSpeed = ");
	sprintf_s(str, "%.4g", speed);
	sEntity1.append(str);
	sEntity1.append(" m/s\n\n");

	//create the  string for entity2
	string sEntity2;
	
	//calculate speed and acceleration
	acceleration = sqrt(entity2->m_acceleration.x*entity2->m_acceleration.x+entity2->m_acceleration.y*entity2->m_acceleration.y);
	speed = sqrt(entity2->m_speed.x*entity2->m_speed.x+entity2->m_speed.y*entity2->m_speed.y);

	//add the name
	sEntity2.append(entity2->m_name);

	//add the acceleration
	sprintf_s(str, "%.4g", acceleration);
	sEntity2.append(" :\nAcceleration = ");
	sEntity2.append(str);
	sEntity2.append(" m/s2");

	//add the speed
	sEntity2.append("\nSpeed = ");
	sprintf_s(str, "%.4g", speed);
	sEntity2.append(str);
	sEntity2.append(" m/s");

	string message = sEntity1;
	message.append(sEntity2);

	CM.LOGMSGBOX().Log()<<message;

	//unpause the simulation
	pauseSimulation(false);

}

bool CPhysics::getCollisionPointBetweenRectangles(CShapeRectangle* entity, CShapeRectangle* entity2)
{
	//get the bounding boxes
	CVector2f* obb1 = entity->getOrientedBoundingBox();
	CVector2f* obb2 = entity2->getOrientedBoundingBox();

	CVector2f intersectionPoint1;
	CVector2f intersectionPoint2;
	bool onePoint = false;
	bool twoPoint = false;
	bool firstPointFromOBB1 = false;

	//check if a point of obb1 is inside obb2
	int i=0;

	//calculate the coordinate of the obb in local axis

	CVector2f orientedBB[4];
	CVector2f orientedPoint;
	calculateOBBAndPointInLocalAxis(obb2, obb1[0], orientedBB, &orientedPoint);

	//get the min and max
	float minX = getMinimum(orientedBB[0].x, orientedBB[1].x);
	minX = getMinimum(orientedBB[2].x, minX);
	minX = getMinimum(orientedBB[3].x, minX);

	float maxX = getMaximum(orientedBB[0].x, orientedBB[1].x);
	maxX = getMaximum(orientedBB[2].x, maxX);
	maxX = getMaximum(orientedBB[3].x, maxX);

	float minY = getMinimum(orientedBB[0].y, orientedBB[1].y);
	minY = getMinimum(orientedBB[2].y, minY);
	minY = getMinimum(orientedBB[3].y, minY);

	float maxY = getMaximum(orientedBB[0].y, orientedBB[1].y);
	maxY = getMaximum(orientedBB[2].y, maxY);
	maxY = getMaximum(orientedBB[3].y, maxY);

	//check if the point is inside
	do
	{
		calculateOBBAndPointInLocalAxis(obb2, obb1[i], orientedBB, &orientedPoint);

		if(orientedPoint.x>=minX && orientedPoint.x<=maxX && orientedPoint.y>=minY && orientedPoint.y<=maxY)
		{
			if(!onePoint)
			{
				intersectionPoint1 = obb1[i];
				onePoint = true;
				firstPointFromOBB1 = true;
			}
			else if(!twoPoint)
			{
				intersectionPoint2 = obb1[i];
				twoPoint = true;
			}
		}

		i++;
	}
	while(i<=3 && !twoPoint);

	if(!twoPoint)//we have to check if a point of obb2 is inside obb1
	{
		calculateOBBAndPointInLocalAxis(obb1, obb2[0], orientedBB, &orientedPoint);

		//get the min and max
		minX = getMinimum(orientedBB[0].x, orientedBB[1].x);
		minX = getMinimum(orientedBB[2].x, minX);
		minX = getMinimum(orientedBB[3].x, minX);

		maxX = getMaximum(orientedBB[0].x, orientedBB[1].x);
		maxX = getMaximum(orientedBB[2].x, maxX);
		maxX = getMaximum(orientedBB[3].x, maxX);

		minY = getMinimum(orientedBB[0].y, orientedBB[1].y);
		minY = getMinimum(orientedBB[2].y, minY);
		minY = getMinimum(orientedBB[3].y, minY);

		maxY = getMaximum(orientedBB[0].y, orientedBB[1].y);
		maxY = getMaximum(orientedBB[2].y, maxY);
		maxY = getMaximum(orientedBB[3].y, maxY);

		//check if the point is inside
		i=0;
		do
		{
			calculateOBBAndPointInLocalAxis(obb1, obb2[i], orientedBB, &orientedPoint);

			if(orientedPoint.x>=minX && orientedPoint.x<=maxX && orientedPoint.y>=minY && orientedPoint.y<=maxY)
			{
				if(!onePoint)
				{
					intersectionPoint1 = obb2[i];
					onePoint = true;
					firstPointFromOBB1 = false;
				}
				else if(!twoPoint)
				{
					intersectionPoint2 = obb2[i];
					twoPoint = true;
				}
			}

			i++;
		}
		while(i<=3 && !twoPoint);
	}

	//calculate the intersection point
	if(twoPoint)
	{
		CVector2f intersectionPoint = (intersectionPoint1 + intersectionPoint2)*0.5;
		CVector2f temp = intersectionPoint1 - intersectionPoint2;
		CVector2f normal(-temp.y, temp.x);
		normal.normalize();

		CCollisionInfo newCollision;
		newCollision.m_collisionPoint = intersectionPoint;
		newCollision.m_collisionNormal = normal;
		entity->m_collisionInfo.push_back(newCollision);
		entity2->m_collisionInfo.push_back(newCollision);

		return true;
	}
	else if (onePoint)
	{
		CCollisionInfo newCollision;
		newCollision.m_collisionPoint = intersectionPoint1;
		if(firstPointFromOBB1)
		{
			int id1 = 0;
			int id2 = 0;
			getClosestEdgeOfPoint(obb2, intersectionPoint1, &id1, &id2);
			CVector2f temp = obb2[id1] - obb2[id2];
			CVector2f normal(-temp.y, temp.x);
			newCollision.m_collisionNormal = normal;
			newCollision.m_collisionNormal.normalize();
		}
		else
		{
			int id1 = 0;
			int id2 = 0;
			getClosestEdgeOfPoint(obb1, intersectionPoint1, &id1, &id2);
			CVector2f temp = obb1[id1] - obb1[id2];
			CVector2f normal(-temp.y, temp.x);
			newCollision.m_collisionNormal = normal;
			newCollision.m_collisionNormal.normalize();
		}

		entity->m_collisionInfo.push_back(newCollision);
		entity2->m_collisionInfo.push_back(newCollision);

		return true;
	}

	return false;
}

void CPhysics::getCollisionPointBetweenRectangleAndCircle(CShapeRectangle* rectangle, CShapeCircle* circle)
{
	//get the bb
	CVector2f* bb = rectangle->getOrientedBoundingBox();

	//get the circle center
	CVector2f center = circle->m_position;

	//get the center and the bb in the obb frame
	CVector2f orientedBB[4];
	CVector2f orientedCenter;
	calculateOBBAndPointInLocalAxis(bb, center, orientedBB, &orientedCenter);

	//get the minimum X
	float bbMinX = getMinimum(orientedBB[0].x, orientedBB[1].x);
	bbMinX = getMinimum(orientedBB[2].x, bbMinX);
	bbMinX = getMinimum(orientedBB[3].x, bbMinX);

	//get the maximum X
	float bbMaxX = getMaximum(orientedBB[0].x, orientedBB[1].x);
	bbMaxX = getMaximum(bbMaxX, orientedBB[2].x);
	bbMaxX = getMaximum(bbMaxX, orientedBB[3].x);

	//get the minimium Y
	float bbMinY = getMinimum(orientedBB[0].y, orientedBB[1].y);
	bbMinY = getMinimum(orientedBB[2].y, bbMinY);
	bbMinY = getMinimum(orientedBB[3].y, bbMinY);

	//get the maximum Y
	float bbMaxY = getMaximum(orientedBB[0].y, orientedBB[1].y);
	bbMaxY = getMaximum(bbMaxY, orientedBB[2].y);
	bbMaxY = getMaximum(bbMaxY, orientedBB[3].y);

	//clamp the center to the bb
	if(orientedCenter.x<bbMinX)
		orientedCenter.x = bbMinX;
	else if(orientedCenter.x>bbMaxX)
		orientedCenter.x = bbMaxX;

	if(orientedCenter.y<bbMinY)
		orientedCenter.y = bbMinY;
	else if(orientedCenter.y>bbMaxY)
		orientedCenter.y = bbMaxY;

	CVector2f intersectionPoint;
	calculatePointInGlobalAxis(bb, orientedCenter, &intersectionPoint);

	CCollisionInfo newCollision;
	newCollision.m_collisionPoint = intersectionPoint;
	CVector2f normal = intersectionPoint - circle->m_position;
	normal.normalize();
	newCollision.m_collisionNormal = normal;

	rectangle->m_collisionInfo.push_back(newCollision);
	circle->m_collisionInfo.push_back(newCollision);


}

void CPhysics::getCollisionPointBetweenCircles(CShapeCircle* circleOne, CShapeCircle* circleTwo)
{
	CVector2f centerOne = circleOne->m_position;
	CVector2f centerTwo = circleTwo->m_position;

	CVector2f line = centerOne - centerTwo;
	line.normalize();

	//circleOne->m_collisionPoint = centerOne + line * circleOne->m_radius;
	CVector2f intersectionPoint = centerTwo + line * (float)circleTwo->m_radius;

	CCollisionInfo newCollision;
	newCollision.m_collisionPoint = intersectionPoint;
	newCollision.m_collisionNormal = line;

	circleTwo->m_collisionInfo.push_back(newCollision);
	circleOne->m_collisionInfo.push_back(newCollision);
}

void CPhysics::getClosestEdgeOfPoint(CVector2f* bb, CVector2f center, int* idPoint1, int* idPoint2)
{
	//calculate the coordinates of the bb and the center in the frame of the bb.
	CVector2f bbInOrientedFrame[4];
	CVector2f newCenter;
	calculateOBBAndPointInLocalAxis(bb, center, bbInOrientedFrame, &newCenter);

	//get the minimum X
	float bbMinX = getMinimum(bbInOrientedFrame[0].x, bbInOrientedFrame[1].x);
	bbMinX = getMinimum(bbInOrientedFrame[2].x, bbMinX);
	bbMinX = getMinimum(bbInOrientedFrame[3].x, bbMinX);

	//get the maximum X
	float bbMaxX = getMaximum(bbInOrientedFrame[0].x, bbInOrientedFrame[1].x);
	bbMaxX = getMaximum(bbMaxX, bbInOrientedFrame[2].x);
	bbMaxX = getMaximum(bbMaxX, bbInOrientedFrame[3].x);

	//get the minimium Y
	float bbMinY = getMinimum(bbInOrientedFrame[0].y, bbInOrientedFrame[1].y);
	bbMinY = getMinimum(bbInOrientedFrame[2].y, bbMinY);
	bbMinY = getMinimum(bbInOrientedFrame[3].y, bbMinY);

	//get the maximum Y
	float bbMaxY = getMaximum(bbInOrientedFrame[0].y, bbInOrientedFrame[1].y);
	bbMaxY = getMaximum(bbMaxY, bbInOrientedFrame[2].y);
	bbMaxY = getMaximum(bbMaxY, bbInOrientedFrame[3].y);

	//clamp the center to the bb
	if(newCenter.x<bbMinX)
		newCenter.x = bbMinX;
	else if(newCenter.x>bbMaxX)
		newCenter.x = bbMaxX;

	if(newCenter.y<bbMinY)
		newCenter.y = bbMinY;
	else if(newCenter.y>bbMaxY)
		newCenter.y = bbMaxY;

	//check if the clamped center is on an existing point
	int idClampedPoint = -1;
	if(newCenter == bbInOrientedFrame[0])
		idClampedPoint = 0;
	else if(newCenter == bbInOrientedFrame[1])
		idClampedPoint = 1;
	else if(newCenter == bbInOrientedFrame[2])
		idClampedPoint = 2;
	else if(newCenter == bbInOrientedFrame[3])
		idClampedPoint = 3;

	//calculate the distance between the clamped point and the points of the bb
	float distance[4];
	for(int i=0; i<4; i++)
	{
		if(i == idClampedPoint)
			continue;

		distance[i] = (bbInOrientedFrame[i] - newCenter).getNorme();
		if(distance[i]<0.001 && idClampedPoint == -1)
			idClampedPoint = i;
	}

	//keep the two closest bounding point
	int id1 = (idClampedPoint+1)%4;
	
	for(int i=0; i<4; i++)
	{
		if(i == idClampedPoint)
			continue;

		if(distance[i]<distance[id1])
			id1 = i;
	}
	int id2 = (id1+1)%4;
	for(int i = 0; i<4; i++)
	{
		if(i==id1 || i == idClampedPoint)
			continue;

		if(distance[i]<distance[id2])
			id2 = i;
	}

	*idPoint1 = id1;
	*idPoint2 = id2;
}

void CPhysics::calculateOBBAndPointInLocalAxis(CVector2f* bb, CVector2f point, CVector2f* newBB, CVector2f* newPoint)
{
	//get one edges and a othogonal edges to make the X and Y axis of the new frame
	CVector2f newXAxis = bb[1] - bb[0];
	newXAxis.normalize();

	CVector2f newYAxis(-newXAxis.y, newXAxis.x);

	//get the angle between X et x
	double cosTeta = newXAxis.dot(CVector2f(1, 0));
	double teta = acos(cosTeta);
	if(newXAxis.y<0)
		teta = 2*3.14 - teta;

	//for the Y axis, the angle is the same plus pie/2
	double tetaPlusPiOverTwo = teta + 3.14 / 2;

	//calculate the new coordinate of the bounding box
	for(int i=0; i<4 ;i++)
	{
		newBB[i].y = (bb[i].y - bb[i].x * (float)tan(teta)) / ((float)sin(tetaPlusPiOverTwo) - (float)cos(tetaPlusPiOverTwo) * (float)tan(teta));
		newBB[i].x = (bb[i].x - newBB[i].y * (float)cos(tetaPlusPiOverTwo))/(float)cos(teta);
	}
	
	//calculate the new coordinates of the point
	newPoint->y = (point.y - point.x * (float)tan(teta)) / ((float)sin(tetaPlusPiOverTwo) - (float)cos(tetaPlusPiOverTwo) * (float)tan(teta));
	newPoint->x = (point.x - newPoint->y * (float)cos(tetaPlusPiOverTwo)) / (float)cos(teta);
}

void CPhysics::calculatePointInGlobalAxis(CVector2f* bb, CVector2f point, CVector2f* newPoint)
{
	//get one edges and a othogonal edges to make the X and Y axis of the new frame
	CVector2f newXAxis = bb[1] - bb[0];
	newXAxis.normalize();

	CVector2f newYAxis(-newXAxis.y, newXAxis.x);

	//get the angle between X et x
	double cosTeta = newXAxis.dot(CVector2f(1, 0));
	double teta = acos(cosTeta);
	if(newXAxis.y<0)
		teta = 2*3.14 - teta;

	//for the Y axis, the angle is the same plus pie/2
	double tetaPlusPiOverTwo = teta + 3.14 / 2;

	//convert the point
	newPoint->x = point.x * (float)cos(teta) + point.y * (float)cos(tetaPlusPiOverTwo);
	newPoint->y = point.y * ((float)sin(tetaPlusPiOverTwo) - (float)cos(tetaPlusPiOverTwo) * (float)tan(teta)) + point.x * (float)tan(teta);
}